'''
Author: Zhuangyu
Date: 2022-11-24 16:51:21
'''

from __future__ import print_function, division
import time
#Importing relevant modules and packages
import numpy as np
import matplotlib.pyplot as plt
import random
from Parameters import *
from scipy import integrate, linalg, interpolate
from tqdm import tqdm
from casadi import *
import mpctools as mpc
import casadi
import scipy.io as sio
from ode_sensitivity_States import ode_sensitivity_States  
import van_Genuchten as vg
from RichardsModel_1D import *
from prejacobian import F1,F2,F3,Fy
from ode_sensitivity import *

np.set_printoptions(precision=4)   # 输出数据精度 保留小数点后4位
np.set_printoptions(linewidth=400) # 输出不换行

#Space and time parameters
Nr,Nt,Nz,dr,dt,dz,Np,Na,Np,N_obs,Hz=circular_parameters()
DeltaT,Nsim=time_parameters()
p=Loam() #Soil parameters

rank_xp = True
# u=np.zeros(20*120)
# uu=np.zeros(Nsim)
# for i in range(20*120):
#     if i < 55:
#         u[i]=-1e-06 #Moisture is supplied only at the first time instant
#     else:
#         u[i]=0
# uu=np.tile(u,(1,60))
# uu=np.transpose(uu)

u=np.zeros(3*120)
uu=np.zeros(Nsim)
for i in range(3*120):
    if i < 5:
        u[i]=-1e-06 #Moisture is supplied only at the first time instant
    else:
        u[i]=0
uu=np.tile(u,(1,60))
uu=np.transpose(uu)


#Initial condition [pressure head]
x0=-1.1*np.ones(Nz)

#Numpy array to store the simulation results [pressure head]
xx=np.zeros((Nsim+1,Nz))
xx[0,:]=x0

ode1=np.zeros((Nsim+1,Nz))

#Model uncertainty
# www1 = 4e-9 # process noise
# vv1 = 8e-7*np.ones(N_obs) # measurement noise
www1 = 4e-9 # process noise
vv1 = 8e-7*np.ones(N_obs) # measurement noise

np.random.seed(2)
w=np.random.normal(0,np.sqrt(www1),[Nsim+1,Nz])
# w=0.01*np.random.randn(Nsim,Nz)
np.random.seed(3)
vv=np.random.randn(Nsim+1,N_obs)*vv1 #measurement noise

#Numpy array to store the simulation results [volumetric moisture content]
yy=np.zeros((Nsim+1,N_obs))
yy[0,:]=getOutputs1D(x0).full().ravel()+vv[0,:]


#unknown inputs
a1=0*np.ones((Nsim+1,Nz))
a=0.3*np.ones((Nsim+1,Nz))

e0 = time.time()

for i in tqdm(range(1, Nsim+1)):
    # Generate observed data (x bias)
    xx[i,:]= DM(ODE_discrete(tuple(xx[i-1]), uu[i-1],a[i-1],DeltaT)).full().ravel()+w[i-1]    
    yy[i,:]=getOutputs1D(xx[i,:]).full().ravel()+vv[i,:]
    

M_known = [0,1,2,3,4,5,6,7,8,9,10,11,12,13,14,15]
M_remain = M_known
M_remove = np.zeros((1,1))
Ny = Nz # Nx:sensor remove      2:注释begin remove
##### initial
print('Consider all the measurements:', M_known)
ysim = np.zeros((Nsim, Ny))
def measurement(x):
    C_all = np.eye(16)
    C = C_all[M_remain,:]
    y = mtimes(C,x)
    return y
for t in range(Nsim):
    C_all = np.eye(16)
    C = C_all[M_remain,:]
    ysim[t,:] = measurement(yy[t,:]).full().ravel()

Flag = ode_sensitivity_States(xx[0:Nsim,:],ysim[0:Nsim,:],uu[0:Nsim,:],a[0:Nsim,:],www1,vv1,rank_xp)
Rank_Sen = Flag[0]
print('Rank of Sensiti:', Rank_Sen)
Sen_state_sort = Flag[1:-1]
print("Sen state sort:",Sen_state_sort)
Sen_deg = Flag[-1]
print("Sen degree:",Sen_deg)


# print("-----------------begin remove---------------")
# for k in range(Nz):
#     Rank_Sen_remain_max = np.zeros((1,1))
#     print(k+1,"-th remove: ")
#     M_remain_try = []
#     M_remain_copy = []
#     Ny = Ny - 1
#     Sen_deg_max = 0
#     for j in range(len(M_remain)):
#         i = M_remain[j]
#         M_remove_try = i;
#         print('try to remove', M_remove_try)
#         M_remain_try[:] = M_remain
#         M_remain_try.remove(i)
#         print('remian these measurements', M_remain_try)
            
#         ysim = np.zeros((Nsim, Ny))
#         def measurement(x):
#             C_all = np.eye(16)
#             C = C_all[M_remain_try,:]
#             y = mtimes(C,x)
#             return y
        
#         for t in range(Nsim):
#             C_all = np.eye(16)
#             C = C_all[M_remain_try,:]
#             ysim[t,:] = measurement(yy[t,:]).full().ravel()
        
#         Flag = ode_sensitivity_States(xx[0:Nsim,:],ysim[0:Nsim,:],uu[0:Nsim,:],a[0:Nsim,:],www1,vv1,rank_xp)
        
#         Rank_Sen_remain = Flag[0]
#         print('Rank of Sensiti:', Rank_Sen_remain)
#         Sen_state_order = Flag[1:-1]
#         print("Sen state order:",Sen_state_order)
#         Sen_deg = Flag[-1]
#         print("Sen degree: %.5g" % Sen_deg)
        
#         if Rank_Sen_remain_max < Rank_Sen_remain:
#             Rank_Sen_remain_max[0,0] = Rank_Sen_remain
            
#         if Sen_deg >= Sen_deg_max:
#            Sen_deg_max = Sen_deg
#            M_remove = M_remove_try
#            M_remain_copy[:] = M_remain_try
#         print("----------------------------------------")
#     if Rank_Sen_remain_max < Nz:
#         print("stop trying")
#         break
#     else:
#         print("new removed:",M_remove)
#         M_remain = M_remain_copy
#         print("remians:",M_remain)
# print("Finially remians:",M_remain)
